//
// Created by lustre on 2023/1/15.
//

#ifndef QUADRUPED_QUADRUPED_H
#define QUADRUPED_QUADRUPED_H
// 舵机的转动范围,对于PCA9685输入的PWM值范围,SG90 MG90S符合此, 更换舵机请修改
#define BASE_POS 150  // 基线
#define END_POS 600   // 上限

// pwm频率 具体舵机具体设置
#define PWM_FREQUENCY 60
// 单步延时，同一个舵机两次转动之间的延时。当上一次动作结束后，定时器才能唤醒执行下一步动作，故需要设置此延时
#define step_delay  150

#include <Adafruit_PWMServoDriver.h>

class Joint {
public:
    // 舵机驱动板的pin 0~15
    int pin;
    // 以下上限不必大于下限，只是在前进的方向上 ，往腿前迈的极限为上限 腿往上提的极限为上限
    // 机械结构转动上限   0 ~ 100
    int upperLimit;
    // 机械结构转动下限   0 ~ 100
    int lowerLimit;

    virtual int getRealUpper() = 0;

    virtual int getRealLower() = 0;

    Joint(int pin, int upperLimit, int lowerLimit);

};

/**
 * 髋关节
 * */
class HipJoint : public Joint {

public:

    // 走路时摆动的范围
    int leftWalkingLimit{};

    int rightWalkingLimit{};

    int realRightLimit{};

    int realLeftLimit{};

    void left();

    void right();

    int getRealLower() override {
        return realRightLimit;
    }

    int getRealUpper() override {
        return realLeftLimit;
    }

    HipJoint(int pin, int leftLimit, int rightLimit);

};

/**
 * 膝关节 与髋关节数据字段并无太大区别 ，但使用不同名称 命名属性和函数便于区分具体操作行为和具体对象
 * */
class KneeJoint : public Joint {

public:

    int realUpperLimit{};

    int realLowerLimit{};

    /**
     * 提腿
     * */
    void up();

    /**
     * 落脚
     * */
    void down();

    int getRealLower() override {
        return realLowerLimit;
    }

    int getRealUpper() override {
        return realUpperLimit;
    }

    KneeJoint(int pin, int upperLimit, int lowerLimit);

};

class Quadruped {


private:
    //左前腿
    HipJoint frontLeftLeg;
    //左前膝盖
    KneeJoint frontLeftKnee;
    //右前腿
    HipJoint rightFrontLeg;
    //右前膝盖
    KneeJoint rightFrontKnee;
    //右后腿
    HipJoint rightRearLeg;
    //右后膝盖
    KneeJoint rightRearKnee;
    //左后腿
    HipJoint rearLeftLeg;
    //左后膝盖
    KneeJoint rearLeftKnee;

    Joint *joints[8] = {&frontLeftLeg,&frontLeftKnee,
                        &rightFrontLeg,&rightFrontKnee,
                        &rightRearLeg,&rightRearKnee,
                        &rearLeftLeg,&rearLeftKnee};


    int volatile quadrupedAction = -1;


    void forward();

    void backward();

    void turnRight();

    void turnLeft();

/**
 * 动作表，这里由Ticker调用
 * 通过 {@link Quadruped#setQuadrupedAction()}
 * 设置具体动作，Ticker在下一次唤醒时调用
 * */
    void (Quadruped::*actionTab[4])() = {
            &Quadruped::forward, &Quadruped::backward,
            &Quadruped::turnRight, &Quadruped::turnLeft
    };

public:


    void init(int sda, int scl);

    /**
     * 当前函数是同步的。需要当前线程异步
     * */
    void doAsyncAction(int action);

    /**
    * @brief 控制转向
    *
    * @param target 目标舵机
    * @param pos pwm值 范围 BASE_POS ~ END_POS
    *
    */
    void turn(uint8_t target, uint16_t pos);

    void stop();

    void reset();

    volatile int getQuadrupedAction() const;

    void setQuadrupedAction(volatile int action);

    void (Quadruped::*allPinActionTab[2])() = {&Quadruped::stop, &Quadruped::reset};

    /**
     *     左前---→右前
     *     ↑        |
     *     |        ↓
     *     左后 ←--- 右后
     * */

    Quadruped(const HipJoint &frontLeftLeg, const KneeJoint &frontLeftKnee, const HipJoint &rightFrontLeg,
              const KneeJoint &rightFrontKnee, const HipJoint &rightRearLeg, const KneeJoint &rightRearKnee,
              const HipJoint &rearLeftLeg, const KneeJoint &rearLeftKnee);
};


#endif //QUADRUPED_QUADRUPED_H
